#include <ros/ros.h>
#include <iostream>
#include <stdio.h>

#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Imu.h>
#include "CameraDriver.h"
#include <signal.h>
#include <fstream>

//#define Calibrate_Camera_Mode
#define Calibrate_IMU_Mode

MakerStereo::CameraDriver stereo_driver;

void shutdownHandler(int sig)
{
    stereo_driver.shutdown();
    ros::shutdown();
    exit(0);
}

int main(int argc,  char ** argv)
{ 
    ros::init(argc, argv, "maker_binocular");
    
    ros::NodeHandle nh;
    
    ros::Publisher  left_image_pub =  nh.advertise<sensor_msgs::Image>("camera/left/image_raw", 10);
    ros::Publisher right_image_pub = nh.advertise<sensor_msgs::Image>("camera/right/image_raw", 10);
    
    ros::Publisher  left_rgb_image_pub =  nh.advertise<sensor_msgs::Image>("left/image_color", 10);
    ros::Publisher right_rgb_image_pub = nh.advertise<sensor_msgs::Image>("right/image_color", 10);
    
    ros::Publisher left_camera_info_pub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info",  10);
    ros::Publisher right_camera_info_pub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info",  10);
    
    ros::Publisher imu_msg_pub = nh.advertise<sensor_msgs::Imu>("/imu", 100);

    // set up stereo camera info
    sensor_msgs::CameraInfo left_camera_info;
    sensor_msgs::CameraInfo right_camera_info;
    sensor_msgs::Imu imu_msg;
    
    left_camera_info.distortion_model = "plumb_bob";
    right_camera_info.distortion_model = "plumb_bob";
        
/*
    // left camera
    /*
    left_camera_info.height = 480;
    left_camera_info.width = 640; 
    left_camera_info.D.push_back(0.144948);
    left_camera_info.D.push_back(-0.353241);
    left_camera_info.D.push_back(-0.002081);
    left_camera_info.D.push_back(0.007011);
    left_camera_info.D.push_back(0.000000);
     
    left_camera_info.K = {701.415048, 0.0, 325.936563, 0.0, 700.805741, 235.991749, 0.0, 0.0, 1.0};
    left_camera_info.R = {0.999240, -0.000872, 0.038958, 0.001172, 0.999970, -0.007668, -0.038950, 0.007708, 0.999211};
    left_camera_info.P = {768.658277, 0, 280.543400, 0,  0, 768.658277, 244.094297, 0,  0, 0, 1, 0};
    left_camera_info.binning_x = 1;
    left_camera_info.binning_y = 1;
    
    // right camera
    right_camera_info.D.push_back(0.065938);
    right_camera_info.D.push_back(0.026545);
    right_camera_info.D.push_back(0.000794);
    right_camera_info.D.push_back(0.004122); 
    right_camera_info.D.push_back(0.000000);
    
    right_camera_info.K = {702.170323, 0.0, 300.267293, 0.0, 702.301011, 253.483819, 0.0, 0.0, 1};
    right_camera_info.R = {0.998917, -0.004869, 0.046274, 0.004513,  0.999959, 0.007796, -0.046310, -0.007579, 0.998898};
    right_camera_info.P = {768.658277, 0, 280.543400 , -64.649990, 0, 768.658277, 244.094297, 0, 0, 0, 1, 0}; 
    right_camera_info.binning_x = 1;
    right_camera_info.binning_y = 1;
*/
    int counter = 0;
    sensor_msgs::Image ros_left_image,  ros_right_image;

   

    vector<MakerStereo::image_msg> image_msgs;
    vector<MakerStereo::imu_msg> imu_msgs;   

    cv::Mat left_image_rgb(480, 640, CV_8UC3, cv::Scalar(0));
    cv::Mat right_image_rgb(480, 640, CV_8UC3, cv::Scalar(0));

    float image_interval;
    float imu_interval[4];
    float acc[12];
    float gyro[12];
    float imu_stamp_s[4];
    float camera_stamp_s;

    ////////////////
    cv_bridge::CvImage img_bridge;
    std_msgs::Header image_header, imu_header;

    image_header.frame_id = "/camera_link";
    imu_header.frame_id = "/imu";

    // set up the initial ros timestamp
    double start_header_s;
    start_header_s = ros::Time::now().toSec();
    std::cout << "start timestamp s :" << start_header_s << std::endl;

    signal(SIGINT, shutdownHandler);

    ros::Rate rate(30);
    
    if (stereo_driver.is_initialized())
    {
        while (true)
        {
            stereo_driver.consume(image_msgs, imu_msgs);

            if (image_msgs.size() > 0)
            {
                // publish ros image msg
                image_header.seq = counter;
                ros::Time image_time;
                image_time.fromSec(start_header_s + camera_stamp_s);
                image_header.stamp = image_time;//ros::Time::fromSec();

                // for gray image,  just publishe it
                for(size_t i = 0; i < image_msgs.size(); i++)
                {
                    if (true)//!m_makerbinocular.is_rgb())
                    {
                        img_bridge = cv_bridge::CvImage(image_header, sensor_msgs::image_encodings::MONO8,  image_msgs[i].left_image());
                        img_bridge.toImageMsg(ros_left_image);
                        left_image_pub.publish(ros_left_image);

                        img_bridge = cv_bridge::CvImage(image_header, sensor_msgs::image_encodings::MONO8,  image_msgs[i].right_image());
                        img_bridge.toImageMsg(ros_right_image);
                        right_image_pub.publish(ros_right_image);
                    }
                    else
                    {
                     //TODO:add rgb support   
                        cv::cvtColor(image_msgs[i].left_image(), left_image_rgb,  CV_GRAY2RGB);
                        cv::cvtColor(image_msgs[i].right_image(), right_image_rgb,  CV_GRAY2RGB);
                    }

                    // give the content of stereo camera info
                    left_camera_info.header = image_header;
                    right_camera_info.header = image_header;

                    left_camera_info_pub.publish(left_camera_info);
                    right_camera_info_pub.publish(right_camera_info);

                    cv::imshow("left image", image_msgs[i].left_image());
                    cv::waitKey(1);
                    cv::imshow("right image" ,  image_msgs[i].right_image());
                    cv::waitKey(1);

                    // publish ros imu msg       
                    for(int j = 0; j < IMAGE_PART; j++)
                    {        
                        imu_header.seq = IMAGE_PART * counter + j;
                        ros::Time imu_time;
                        imu_time.fromSec(imu_msgs[j].ts());
                        //= ros::Time::fromSec(start_header_s + imu_stamp_s[i]);
                        imu_header.stamp = imu_time;
                        imu_msg.header = imu_header;
                        imu_msg.linear_acceleration.x = imu_msgs[j].acc_x();
                        imu_msg.linear_acceleration.y = imu_msgs[j].acc_y();
                        imu_msg.linear_acceleration.z = imu_msgs[j].acc_z();
                        imu_msg.angular_velocity.x = imu_msgs[j].gyro_x();
                        imu_msg.angular_velocity.y = imu_msgs[j].gyro_y();
                        imu_msg.angular_velocity.z = imu_msgs[j].gyro_z();
                        imu_msg_pub.publish(imu_msg);
                    }
                    counter++;
                }
            }
            rate.sleep();
        }
    }

    return 0;
}
